#include "vehicle_linear_4d4s.h"

namespace auto_ros
{
namespace control
{
VehicleLinear4d4s::VehicleLinear4d4s(std::string dyn_yaml_path_name)
	: VehicleDynBase(dyn_yaml_path_name, 5, 6, 5, 3)

{
	log_map_["ax_max_fl"] = 2.0;
	log_map_["ax_max_fr"] = 2.0;
	log_map_["ax_max_rl"] = 2.0;
	log_map_["ax_max_rr"] = 2.0;
	mass_ = dyn_yaml_["mass"].as<double>();
	cf_ = dyn_yaml_["cf"].as<double>();
	cr_ = dyn_yaml_["cr"].as<double>();
	lf_ = dyn_yaml_["lf"].as<double>();
	lr_ = dyn_yaml_["lr"].as<double>();
	iz_ = dyn_yaml_["iz"].as<double>();
	h_ = dyn_yaml_["h"].as<double>();
	lw_ = dyn_yaml_["lw"].as<double>();
	road_mu_ = dyn_yaml_["road_mu"].as<double>();
	max_lateral_acc_ = dyn_yaml_["max_lateral_acc"].as<double>();
	max_tire_f_angle_ = dyn_yaml_["max_tire_f_angle"].as<double>();
	speed_threshold_ = dyn_yaml_["speed_threshold"].as<double>();

	c_yaw_rate_ = -lf_ / 1.0 * cf_ + lr_ / 1.0 * cr_; //speed realated
	c_beta_ = -(cf_ + cr_);
	c_delta_f_ = cf_;
	c_delta_r_ = cr_;

	d_yaw_rate_ = -lf_ * lf_ / 1.0 * cf_ - lr_ * lr_ / 1.0 * cr_; //speed related
	d_beta_ = -lf_ * cf_ + lr_ * cr_;
	d_delta_f_ = lf_ * cf_;
	d_delta_r_ = -lr_ * cr_;
	d_l_ = lw_ * mass_ / 2 / iz_;
	speed_threshold_ = 1.0;
	//init speed is 1.0m/s
	speed_ = 1.0;
	state_matrix_ << 0, 0, 0, 0, 0,
		0, c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_ - 1.0, 0, 0,
		0, d_beta_ / iz_ / 1.0, d_yaw_rate_ / iz_, 0, 0,
		0, 1, 0, 0, 1.0,
		0, 0, 1, 0, 0;
	control_matrix_ << 0, 0, 1, 1, 1, 1,
		c_delta_f_ / mass_, c_delta_r_ / mass_, 0, 0, 0, 0,
		d_delta_f_ / iz_, d_delta_r_ / iz_, -d_l_, d_l_, -d_l_, d_l_,
		0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0;
	distur_matrix_ << 0, -1, 1, 0, 0,
		0, 0, 0, 1, 0,
		0, 0, 0, 0, 1,
		0, 0, 0, 0, 0,
		-1.0, 0, 0, 0, 0;
	out_state_matrix_ << 1, 0, 0, 0, 0,
		0, 0, 0, 1, 0,
		0, 0, 0, 0, 1;
	out_distur_matrix_ << 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0,
		0, 0, 0, 0, 0;
	if (init_ref_solver() == false)
	{
		std::cout << "\033[31m VehicleLinear4d4s::init_ref_solver failed. \033[0m" << std::endl;
		throw 100;
	};
}
bool VehicleLinear4d4s::init_ref_solver()
{
	int dae_num = 4;
	int var_num = state_dim_ + control_dim_ + dae_num;
	int start_dae_var = state_dim_ + control_dim_;
	int constriants_num = state_dim_ + out_dim_ + dae_num;
	int start_dae_constraints = state_dim_ + out_dim_;

	hessian_ = Eigen::MatrixXd::Zero(var_num, var_num);
	gradient_ = Eigen::VectorXd::Zero(var_num);
	hessian_(state_dim_ + 2, state_dim_ + 2) = std::pow(1 / 1.0, 2);
	hessian_(state_dim_ + 3, state_dim_ + 3) = std::pow(1 / 1.0, 2);
	hessian_(state_dim_ + 4, state_dim_ + 4) = std::pow(1 / 1.0, 2);
	hessian_(state_dim_ + 5, state_dim_ + 5) = std::pow(1 / 1.0, 2);

	double alpha_max = dyn_yaml_["alpha_max"].as<double>() * M_PI / 180.0;
	double alpha_max_sqr = std::pow(alpha_max, 2);

	hessian_(start_dae_var, start_dae_var) = 1 / alpha_max_sqr;
	hessian_(start_dae_var + 1, start_dae_var + 1) = 1 / alpha_max_sqr;
	hessian_(start_dae_var + 2, start_dae_var + 2) = 1 / alpha_max_sqr;
	hessian_(start_dae_var + 3, start_dae_var + 3) = 1 / alpha_max_sqr;

	constraint_matrix_ = Eigen::MatrixXd::Zero(constriants_num, var_num);

	constraint_matrix_.block(0, 0, state_dim_, state_dim_) = state_matrix_;
	constraint_matrix_.block(0, state_dim_, state_dim_, control_dim_) = control_matrix_;
	constraint_matrix_.block(state_dim_, 0, out_dim_, state_dim_) = out_state_matrix_;
	constraint_matrix_.block(state_dim_, state_dim_, out_dim_, control_dim_) =
		Eigen::MatrixXd::Zero(out_dim_, control_dim_);

	constraint_matrix_(start_dae_constraints, 1) = 1 / 1.0;
	constraint_matrix_(start_dae_constraints, 2) = lf_ / 1.0;
	constraint_matrix_(start_dae_constraints, 5) = -1;
	constraint_matrix_(start_dae_constraints + 1, 1) = 1 / 1.0;
	constraint_matrix_(start_dae_constraints + 1, 2) = lf_ / 1.0;
	constraint_matrix_(start_dae_constraints + 1, 5) = -1;

	constraint_matrix_(start_dae_constraints + 2, 1) = 1 / 1.0;
	constraint_matrix_(start_dae_constraints + 2, 2) = -lr_ / 1.0;
	constraint_matrix_(start_dae_constraints + 2, 6) = -1;

	constraint_matrix_(start_dae_constraints + 3, 1) = 1 / 1.0;
	constraint_matrix_(start_dae_constraints + 3, 2) = -lr_ / 1.0;
	constraint_matrix_(start_dae_constraints + 3, 6) = -1;

	lowerBound_ = Eigen::VectorXd::Zero(constriants_num);
	upperBound_ = Eigen::VectorXd::Zero(constriants_num);
	// settings
	ref_qp_solver_.settings()->setVerbosity(true);
	ref_qp_solver_.settings()->setWarmStart(true);
	ref_qp_solver_.settings()->setPolish(true);
	// set the initial data of the QP ref_qp_solver_
	ref_qp_solver_.data()->setNumberOfVariables(var_num);
	ref_qp_solver_.data()->setNumberOfConstraints(constriants_num);
	ref_qp_solver_.data()->clearHessianMatrix();
	Eigen::SparseMatrix<double> spar_hessian = hessian_.sparseView();
	if (!ref_qp_solver_.data()->setHessianMatrix(spar_hessian))
		return false;
	ref_qp_solver_.data()->clearLinearConstraintsMatrix();
	Eigen::SparseMatrix<double> spar_constraint_matrix = constraint_matrix_.sparseView();
	if (!ref_qp_solver_.data()->setLinearConstraintsMatrix(spar_constraint_matrix))
		return false;
	if (!ref_qp_solver_.data()->setGradient(gradient_))
		return false;
	if (!ref_qp_solver_.data()->setLowerBound(lowerBound_))
		return false;
	if (!ref_qp_solver_.data()->setUpperBound(upperBound_))
		return false;
	if (!ref_qp_solver_.initSolver())
		return false;
	return true;
}
void VehicleLinear4d4s::para_update_matrix(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}
	speed_ = speed;
	c_yaw_rate_ = -lf_ / speed * cf_ + lr_ / speed * cr_;
	d_yaw_rate_ = -lf_ * lf_ / speed * cf_ - lr_ * lr_ / speed * cr_;

	state_matrix_ << 0, 0, 0, 0, 0,
		0, c_beta_ / mass_ / speed, c_yaw_rate_ / mass_ - speed, 0, 0,
		0, d_beta_ / iz_ / speed, d_yaw_rate_ / iz_, 0, 0,
		0, 1, 0, 0, speed,
		0, 0, 1, 0, 0;
	distur_matrix_(4, 0) = -speed;
}
bool VehicleLinear4d4s::calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &state_ref, Eigen::VectorXd &control_ref,
	std::map<std::string, double> other_par_map)
{
	double ax;
	double ay;
	auto ax_iter = other_par_map.find("ax");
	if (other_par_map.end() != ax_iter)
	{
		ax = ax_iter->second;
	}
	else
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::calc_steady_state_ctr ax is not in other_par_map . \033[0m" << std::endl;
		throw 100;
	}
	auto ay_iter = other_par_map.find("ay");
	if (other_par_map.end() != ay_iter)
	{
		ay = ay_iter->second;
	}
	else
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::calc_steady_state_ctr ay is not in other_par_map . \033[0m" << std::endl;
		throw 100;
	}
	//////////////////////////////////////////////////////////////////////////////////
	double l = lf_ + lr_;
	double s_coff = g / 2.0 / l;
	double ax_coff = h_ / 2.0 / l;
	double ay_coff = h_ / 2.0 / lw_ / l;
	double az_fl = lr_ * (s_coff - ay * ay_coff) - ax * ax_coff;
	double ax_fl_max = road_mu_ * az_fl;
	double az_fr = lr_ * (s_coff + ay * ay_coff) - ax * ax_coff;
	double ax_fr_max = road_mu_ * az_fr;
	double az_rl = lf_ * (s_coff - ay * ay_coff) + ax * ax_coff;
	double ax_rl_max = road_mu_ * az_rl;
	double az_rr = lf_ * (s_coff + ay * ay_coff) + ax * ax_coff;
	double ax_rr_max = road_mu_ * az_rr;
	Eigen::VectorXd ax_max_vec(4);
	ax_max_vec << ax_fl_max, ax_fr_max, ax_rl_max, ax_rr_max;

	int dae_num = 4;
	int var_num = state_dim_ + control_dim_ + dae_num;
	int start_dae_var = state_dim_ + control_dim_;
	int constriants_num = state_dim_ + out_dim_ + dae_num;
	int start_dae_constraints = state_dim_ + out_dim_;

	hessian_ = Eigen::MatrixXd::Zero(var_num, var_num);
	gradient_ = Eigen::VectorXd::Zero(var_num);
	hessian_(state_dim_ + 2, state_dim_ + 2) = std::pow(1 / ax_fl_max, 2);
	hessian_(state_dim_ + 3, state_dim_ + 3) = std::pow(1 / ax_fr_max, 2);
	hessian_(state_dim_ + 4, state_dim_ + 4) = std::pow(1 / ax_rl_max, 2);
	hessian_(state_dim_ + 5, state_dim_ + 5) = std::pow(1 / ax_rr_max, 2);

	double alpha_max = dyn_yaml_["alpha_max"].as<double>() * M_PI / 180.0;
	double alpha_max_sqr = std::pow(alpha_max, 2);

	hessian_(start_dae_var, start_dae_var) = 1 / alpha_max_sqr;
	hessian_(start_dae_var + 1, start_dae_var + 1) = 1 / alpha_max_sqr;
	hessian_(start_dae_var + 2, start_dae_var + 2) = 1 / alpha_max_sqr;
	hessian_(start_dae_var + 3, start_dae_var + 3) = 1 / alpha_max_sqr;

	constraint_matrix_ = Eigen::MatrixXd::Zero(constriants_num, var_num);

	constraint_matrix_.block(0, 0, state_dim_, state_dim_) = state_matrix_;
	constraint_matrix_.block(0, state_dim_, state_dim_, control_dim_) = control_matrix_;
	constraint_matrix_.block(state_dim_, 0, out_dim_, state_dim_) = out_state_matrix_;
	constraint_matrix_.block(state_dim_, state_dim_, out_dim_, control_dim_) =
		Eigen::MatrixXd::Zero(out_dim_, control_dim_);

	constraint_matrix_(start_dae_constraints, 1) = 1 / speed_;
	constraint_matrix_(start_dae_constraints, 2) = lf_ / speed_;
	constraint_matrix_(start_dae_constraints, 5) = -1;
	constraint_matrix_(start_dae_constraints, start_dae_var) = -1;

	constraint_matrix_(start_dae_constraints + 1, 1) = 1 / speed_;
	constraint_matrix_(start_dae_constraints + 1, 2) = lf_ / speed_;
	constraint_matrix_(start_dae_constraints + 1, 5) = -1;
	constraint_matrix_(start_dae_constraints + 1, start_dae_var + 1) = -1;

	constraint_matrix_(start_dae_constraints + 2, 1) = 1 / speed_;
	constraint_matrix_(start_dae_constraints + 2, 2) = -lr_ / speed_;
	constraint_matrix_(start_dae_constraints + 2, 6) = -1;
	constraint_matrix_(start_dae_constraints + 2, start_dae_var + 2) = -1;

	constraint_matrix_(start_dae_constraints + 3, 1) = 1 / speed_;
	constraint_matrix_(start_dae_constraints + 3, 2) = -lr_ / speed_;
	constraint_matrix_(start_dae_constraints + 3, 6) = -1;
	constraint_matrix_(start_dae_constraints + 3, start_dae_var + 3) = -1;

	lowerBound_ = Eigen::VectorXd::Zero(constriants_num);
	upperBound_ = Eigen::VectorXd::Zero(constriants_num);
	lowerBound_.block(0, 0, state_dim_, 1) = -distur_matrix_ * distur_ref;
	/*std::cout << "-distur_matrix_ * distur_ref is:" << std::endl
			  << -distur_matrix_ * distur_ref << std::endl;*/

	if (out_distur_matrix_.rows() != 0)
	{
		lowerBound_.block(state_dim_, 0, out_dim_, 1) = y_controlled_ref - out_distur_matrix_ * distur_ref;
	}
	else
	{
		lowerBound_.block(state_dim_, 0, out_dim_, 1) = y_controlled_ref;
	}
	upperBound_ = lowerBound_;
	Eigen::SparseMatrix<double> spar_hessian = hessian_.sparseView();
	Eigen::SparseMatrix<double> spar_constraint_matrix = constraint_matrix_.sparseView();
	if (!ref_qp_solver_.updateHessianMatrix(spar_hessian))

	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::qp_get_ref::ref_qp_solver_.updateHessianMatrix() failed. \033[0m" << std::endl;
		return false;
	}
	if (!ref_qp_solver_.updateLinearConstraintsMatrix(spar_constraint_matrix))
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::qp_get_ref::ref_qp_solver_.updateLinearConstraintsMatrix() failed. \033[0m" << std::endl;
		return false;
	}
	if (!ref_qp_solver_.updateGradient(gradient_))
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::qp_get_ref::ref_qp_solver_.updateGradient() failed. \033[0m" << std::endl;
		return false;
	}
	if (!ref_qp_solver_.updateBounds(lowerBound_, upperBound_))
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::qp_get_ref::ref_qp_solver_.updateBounds() failed. \033[0m" << std::endl;
		return false;
	}
	if (!ref_qp_solver_.solve())
	{
		std::cout << "\033[31m Error::VehicleLinear4d4s::qp_get_ref::ref_qp_solver_.solve() failed. \033[0m" << std::endl;
		return false;
	}
	else
	{
		Eigen::VectorXd sol_ = ref_qp_solver_.getSolution();
		std::cout << "sol_ is:" << std::endl
				  << sol_ << std::endl;
		state_ref = sol_.block(0, 0, state_dim_, 1);
		control_ref = sol_.block(state_dim_, 0, control_dim_, 1);
		Eigen::VectorXd alpha_vec = sol_.block(start_dae_var, 0, 4, 1);

		Eigen::VectorXd tire_utily_vec(4);
		std::vector<std::string> tire_index_str_vec = {"fl", "fr", "rl", "rr"};
		tire_utily_vec = Eigen::VectorXd::Zero(4);

		for (int tire_index = 0; tire_index < 4; tire_index++)
		{
			log_map_["ax_max" + tire_index_str_vec[tire_index]] = ax_max_vec(tire_index);
			tire_utily_vec(tire_index) = std::pow(alpha_vec(tire_index) * alpha_vec(tire_index) / alpha_max_sqr +
													  std::pow(control_ref(2 + tire_index) / ax_max_vec(tire_index), 2),
												  0.5);
		}
		std::cout << "tire utily is:" << std::endl;
		std::cout << tire_utily_vec << std::endl;
		log_map_;
	}
	return true;
}

double VehicleLinear4d4s::mass()
{
	return mass_;
}
double VehicleLinear4d4s::lf()
{
	return lf_;
}
double VehicleLinear4d4s::lr()
{
	return lr_;
}
double VehicleLinear4d4s::cf()
{
	return cf_;
}
double VehicleLinear4d4s::cr()
{
	return cr_;
}
double VehicleLinear4d4s::iz()
{
	return iz_;
}
double VehicleLinear4d4s::h()
{
	return h_;
}
double VehicleLinear4d4s::lw()
{
	return lw_;
}
double VehicleLinear4d4s::max_tire_f_angle()
{
	return max_tire_f_angle_;
}
double VehicleLinear4d4s::speed_threshold()
{
	return speed_threshold_;
}

} // namespace control
} // namespace auto_ros